%% Ackermann Model MPC Control with Figure-8 Trajectory Data Collection
% 作者: magical857
% 日期: 2025-11-01
% 描述: 使用MPC控制器让Ackermann车辆跟踪8字轨迹并采集数据

clear; clc; close all;

%% 添加路径
addpath('models');
addpath('controllers');
addpath('trajectories');
addpath('utils');

%% 参数设置
params = ackermann_params();

% 仿真参数
dt = 0.1;              % 采样时间 (s)
T_total = 60;          % 总仿真时间 (s)
t = 0:dt:T_total;      % 时间向量
N_steps = length(t);   % 仿真步数

%% 生成8字轨迹
fprintf('生成8字参考轨迹...\n');
[x_ref, y_ref, theta_ref, v_ref] = generate_figure8(t, params);

%% 初始化状态
x0 = [x_ref(1); y_ref(1); theta_ref(1); v_ref(1)];  % [x, y, theta, v]
x = x0;

% 数据存储
X_history = zeros(4, N_steps);     % 状态历史
U_history = zeros(2, N_steps-1);   % 控制历史
X_ref = [x_ref; y_ref; theta_ref; v_ref];

%% 设置MPC控制器
fprintf('设置MPC控制器...\n');
mpc_controller = setup_mpc(params, dt);

%% 仿真循环
fprintf('开始仿真...\n');
fig = figure('Position', [100, 100, 1200, 800]);

for k = 1:N_steps-1
    % 当前参考状态
    ref = X_ref(:, k);
    
    % MPC控制
    u = mpc_control_step(mpc_controller, x, ref, params);
    
    % 系统动力学更新
    x_next = ackermann_dynamics(x, u, dt, params);
    
    % 存储数据
    X_history(:, k) = x;
    U_history(:, k) = u;
    
    % 更新状态
    x = x_next;
    
    % 实时可视化 (每10步更新一次)
    if mod(k, 10) == 0
        plot_trajectory(X_history(:, 1:k), X_ref, k, fig);
        drawnow;
        fprintf('进度: %d/%d (%.1f%%)\n', k, N_steps-1, 100*k/(N_steps-1));
    end
end

% 保存最后一个状态
X_history(:, N_steps) = x;

fprintf('仿真完成！\n');

%% 保存数据
fprintf('保存数据到文件...\n');
data_struct = struct();
data_struct.time = t;
data_struct.states = X_history;           % 4 x N
data_struct.controls = U_history;         % 2 x (N-1)
data_struct.reference = X_ref;            % 4 x N
data_struct.params = params;
data_struct.dt = dt;

save_data(data_struct, '../data/raw/trajectory_data.mat');
fprintf('数据已保存到: ../data/raw/trajectory_data.mat\n');

%% 最终可视化
figure('Position', [100, 100, 1400, 900]);

% 子图1: XY轨迹
subplot(3, 3, [1, 2, 4, 5]);
plot(X_ref(1,:), X_ref(2,:), 'b--', 'LineWidth', 2, 'DisplayName', '参考轨迹');
hold on;
plot(X_history(1,:), X_history(2,:), 'r-', 'LineWidth', 1.5, 'DisplayName', '实际轨迹');
grid on; axis equal;
xlabel('X (m)'); ylabel('Y (m)');
title('8字轨迹跟踪');
legend('Location', 'best');

% 子图2: 速度
subplot(3, 3, 3);
plot(t, v_ref, 'b--', 'LineWidth', 2); hold on;
plot(t, X_history(4,:), 'r-', 'LineWidth', 1.5);
grid on; xlabel('时间 (s)'); ylabel('速度 (m/s)');
title('速度曲线'); legend('参考', '实际');

% 子图3: 航向角
subplot(3, 3, 6);
plot(t, theta_ref, 'b--', 'LineWidth', 2); hold on;
plot(t, X_history(3,:), 'r-', 'LineWidth', 1.5);
grid on; xlabel('时间 (s)'); ylabel('航向角 (rad)');
title('航向角曲线'); legend('参考', '实际');

% 子图4: 加速度控制
subplot(3, 3, 7);
stairs(t(1:end-1), U_history(1,:), 'k-', 'LineWidth', 1.5);
grid on; xlabel('时间 (s)'); ylabel('加速度 (m/s²)');
title('控制输入: 加速度');
ylim([params.a_min-0.5, params.a_max+0.5]);

% 子图5: 转角控制
subplot(3, 3, 8);
stairs(t(1:end-1), rad2deg(U_history(2,:)), 'k-', 'LineWidth', 1.5);
grid on; xlabel('时间 (s)'); ylabel('转角 (度)');
title('控制输入: 前轮转角');
ylim([rad2deg(params.delta_min)-5, rad2deg(params.delta_max)+5]);

% 子图6: 跟踪误差
subplot(3, 3, 9);
pos_error = sqrt((X_history(1,:) - X_ref(1,:)).^2 + (X_history(2,:) - X_ref(2,:)).^2);
plot(t, pos_error, 'r-', 'LineWidth', 1.5);
grid on; xlabel('时间 (s)'); ylabel('位置误差 (m)');
title('位置跟踪误差');

saveas(gcf, '../results/figures/matlab_simulation_results.png');
fprintf('可视化结果已保存！\n');